#pragma once
#include <limits>
#include <vector>
#include <queue>
#include <map>
#include <cmath>
#include <std_msgs/msg/float32.hpp>

struct TableBP{
    std::vector<float> BP;
    std::vector<float> Table;
};

class Tool{
    public:
        float DivideProtect(float value);
        float LookUpTable(float find_value ,TableBP table_bp);
        float Satuation(float value, float min, float max);
        float Integrate(int id, float value, float min, float max, float hz, bool reset_flag); //now use id: 1 2
        float Derivative(int id, float value, float min, float max, float hz, bool reset_flag); //now use id: 1
        float ButterworthFilter(int id, float value, float wc, float hz); //now use id: 1 2
        float Delay(int id, float value, int period); //now use id: 1
        float RateLimit(int id, float value, float max_rate, float hz); //now use id: 1
        float StepUp(int period, float start_value, float end_value);
    private:
        std::map<int, float> integrate_value;
        std::map<int, std::queue<float>> derivative_input;
        std::map<int, std::queue<float>> delay_input;
        std::map<int, float> filter_output;
        std::map<int, float> rate_limit_output;
        int step_up_value = -1;
};